#include "pid.h"


float KP = 100.0f; 
float KI = 150.0f; 
float KD =  0.0f;		
tracing_pid tracing_pid1={(double)0.31,100.0f,0.00f};
//0为循迹,1为直线
 int pid_mode = 0;



short Line_PID_Control(_Moto_Str* MotoStr)
{
	float Error   = 0;
	float P_Error = 0;
	float I_Error = 0;
	float D_Error = 0;
	float add     = 0;
	
	Error = MotoStr->Target_Speed  - MotoStr->Current_Speed;
	//Update the current one-two third-order error for the current proportional integral and differential calculation
	P_Error = Error;
	I_Error = Error - MotoStr->L_Error;
	D_Error = Error - 2*MotoStr->L_Error + MotoStr->LL_Error;

	//calculation current proportional integral and differential 
	add = KP * P_Error + KI * I_Error + KD * D_Error;
	MotoStr->ESC_Output_PWM += add;

	MotoStr->LL_Error = MotoStr->L_Error;
	MotoStr->L_Error = Error;

	if(MotoStr->ESC_Output_PWM > ESC_output_PWM_LIMT)	MotoStr->ESC_Output_PWM = ESC_output_PWM_LIMT;		
	else if(MotoStr->ESC_Output_PWM < 0)	MotoStr->ESC_Output_PWM = 0;
	
	return MotoStr->ESC_Output_PWM;
}
//返回速度差值
float Tracing_PID_Control(tracing_Str* tracing)
{
	tracing->P = tracing->error;
	tracing->I += tracing->error;
	tracing->D = tracing->error - tracing->Previous_Error;
	tracing->pid_value = tracing->P * tracing_pid1.Kp + tracing->I * tracing_pid1.Ki + tracing->D * tracing_pid1.Kd;
	tracing->Previous_Error = tracing->error;
	return tracing->pid_value;
}


		

